/*
 * MathHelper.cpp
 * 
 * Created January 20th, 2012
 *
 * Purpose: To serve as static class, to be used for Math calculations, without the need of using class objects.
 *
 */

#include "MathHelper.h"

namespace Physics
{
	float MathHelper::sqrDistancePointToCube(Vector3 &point, const Box3D &box)
	{
		float x0 = box.getCenter().getX() - (box.getWidth() / 2);
		float x1 = box.getCenter().getX() + (box.getWidth() / 2);
		float y0 = box.getCenter().getY() - (box.getHeight() / 2);
		float y1 = box.getCenter().getY() + (box.getHeight() / 2);
		float z0 = box.getCenter().getZ() - (box.getLength() / 2);
		float z1 = box.getCenter().getZ() + (box.getLength() / 2);

		Vector3 closest = box.getCenter();

		if(point.getX() < x0) { closest.setX(x0); }
		else if(point.getX() > x1) { closest.setX(x1); }
		else { closest.setX(point.getX()); }

		if(point.getY() < y0) { closest.setY(y0); }
		else if(point.getY() > y1) { closest.setY(y1); }
		else { closest.setY(point.getY()); }

		if(point.getZ() < z0) { closest.setZ(z0); }
		else if(point.getZ() > z1) { closest.setZ(z1); }
		else { closest.setZ(point.getZ()); }

		Vector3 ray = point - closest;

		float sqrDist = ray.dotProduct(ray);

		return sqrDist;

	}

	float MathHelper::sqrDistancePointToBox(Vector2 &point, const Box2D &box)
	{
		float x0 = box.getCenter().getX() - (box.getWidth() / 2);
		float x1 = box.getCenter().getX() + (box.getWidth() / 2);
		float y0 = box.getCenter().getY() - (box.getHeight() / 2);
		float y1 = box.getCenter().getY() + (box.getHeight() / 2);

		Vector2 closest = box.getCenter();

		if(point.getX() <= x0) { closest.setX(x0); }
		else if(point.getX() >= x1) { closest.setX(x1); }
		else { closest.setX(point.getX()); }

		if(point.getY() <= y0) { closest.setY(y0); }
		else if(point.getY() >= y1) { closest.setY(y1); }
		else { closest.setY(point.getY()); }

		Vector2 ray = point - closest;

		float sqrDist = ray.dotProduct(ray);

		return sqrDist;
	}

	float MathHelper::squared(float const &value)
	{
		return (value * value);
	}

	float MathHelper::findDistance(Vector2 const &lhs, Vector2 const &rhs)
	{
		// sqrt( ( vec2.x - vec1.x )^2 + (vec2.y - vec1.y )^2 )
		return sqrt( squared( rhs.getX() - lhs.getX() ) +  squared( rhs.getY() - lhs.getY() ) );
	}

	float MathHelper::findDistance(Vector3 const &lhs, Vector3 const &rhs)
	{
		return sqrt( squared( rhs.getX() - lhs.getX() ) + squared( rhs.getY() - lhs.getY() ) +  squared( rhs.getZ() - lhs.getZ() ));
	}
	
	float MathHelper::dotProduct(Vector2 const &lhs, Vector2 const &rhs)
	{
		float tempX, tempY;

		tempX = lhs.getX() * rhs.getX();
		tempY = lhs.getY() * rhs.getY();

		return (tempX + tempY);
	}

	float MathHelper::dotProduct(Vector3 const &lhs, Vector3 const &rhs)
	{
		float tempX, tempY, tempZ;

		tempX = lhs.getX() * rhs.getX();
		tempY = lhs.getY() * rhs.getY();
		tempZ = lhs.getZ() * rhs.getZ();

		return (tempX + tempY + tempZ);
	}

	float MathHelper::lengthSquared(Vector2 const &vec)
	{
		return dotProduct(vec, vec);
	}

	float MathHelper::lengthSquared(Vector3 const &vec)
	{
		return dotProduct(vec, vec);
	}

	float MathHelper::degreesToRadians(const float angleDeg)
	{
		return (float)(angleDeg * (PI / 180) );
	}

	float MathHelper::radiansToDegrees(const float angleRad)
	{
		return (float)(angleRad * (180 / PI) );
	}

	Vector2 MathHelper::degreesToVector2(const float angleDeg)
	{
		float angleRad = degreesToRadians(angleDeg);
		return Vector2( cos(angleRad), sin(angleRad) );
	}

	Vector2 MathHelper::radiansToVector2(const float angleRad)
	{
		return Vector2( cos(angleRad), sin(angleRad) );
	}

	//Vector3 MathHelper::degreesToVector3(const float angleDeg)
	//{
		//====>> 2B cont-d from here <<===
	//	float angleRad = degreesToRadians(angleDeg);
	//	return Vector3( cos(angleRad), sin(angleRad) );
	//}
	
	/*Vector3 MathHelper::radiansToVector3(const float angleRad)
	{
		Vector3 x;
		return x;
	}*/


	//Magnitude: sqrt (x^2 + y^2)
	float MathHelper::magnitude(Vector2 const &vec)
	{
		return (sqrt( lengthSquared(vec) ));
	}

	//Magnitude: sqrt (x^2 + y^2 + z^2)
	float MathHelper::magnitude(Vector3 const &vec)
	{
		//return (sqrt( (pow(rhs.x,2)) + (pow(rhs.y,2)) + (pow(rhs.z,2))) );
		//sqrt & pow are expensive calls so we use primitive ways to solve
		return ( sqrt( lengthSquared(vec) ));

	}

	Vector2 MathHelper::normal(Vector2 const &vec)
	{
		return Vector2(vec.getX() / magnitude(vec), vec.getY() / magnitude(vec));
	}

	Vector3 MathHelper::normal(Vector3 const &vec)
	{
		return Vector3(vec.getX() / magnitude(vec), vec.getY() / magnitude(vec), vec.getZ() / magnitude(vec));
	}

	/*
	x	y	z
	lhs	x1	y1	z1
	rhs	x2	y2	z2
	--------------------
	[ (y1)(z2) - (y2)(z1) ] - [ (x1)(z2) - (x2)(z1) ] + [ (x1)(y2) - (x2)(y1) ]
	 */
	Vector3 MathHelper::crossProduct(Vector3 const &lhs, Vector3 const &rhs)
	{
		float x, y, z;

		x = (lhs.getY() * rhs.getZ()) - (rhs.getY() * lhs.getZ());
		y = (-1) * ((lhs.getX() * rhs.getZ()) - (rhs.getX() * lhs.getZ()));
		z = (lhs.getX() * rhs.getY()) - (rhs.getX() * lhs.getY());

		return Vector3(x, y, z);

	}
} /* NAMESPACE */